// -*- mode: c++; indent-tabs-mode: nil; -*-
//
//The Biomolecule Toolkit (BTK) is a C++ library for use in the
//modeling, analysis, and design of biological macromolecules.
//Copyright (C) 2001-2006, Tim Robertson <kid50@users.sourceforge.net>,
//                         Chris Saunders <ctsa@users.sourceforge.net>
//
//This program is free software; you can redistribute it and/or modify
//it under the terms of the GNU Lesser General Public License as published
//by the Free Software Foundation; either version 2.1 of the License, or (at
//your option) any later version.
//
//This program is distributed in the hope that it will be useful,  but
//WITHOUT ANY WARRANTY; without even the implied warranty of
//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
//Lesser General Public License for more details.
//
//You should have received a copy of the GNU Lesser General Public License
//along with this program; if not, write to the Free Software Foundation,
//Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
//

#include <iostream>
#include <limits>

#include <btk/core/math/linear_algebra.hpp>

using namespace std;
using namespace BTK;
using namespace BTK::MATH;

double
BTK::MATH::
point_dihedral(BTKVector const & a,
               BTKVector const & b,
               BTKVector const & c,
               BTKVector const & d)
{
  BTKVector ba(a-b);
  BTKVector bc(c-b);
  BTKVector cd(d-c);
  return vector_dihedral(ba,bc,cd);
}

bool
BTK::MATH::
within_sqr_dist(BTKVector const & a,
                BTKVector const & b,
                double r_squared)
{
  double sum,diff;

  diff=a[0]-b[0];
  sum=diff*diff;
  if(sum > r_squared)  // If the x distance is already too big, we can avoid
    return false;  // unecessary calculations.
  diff=a[1]-b[1];
  sum+=diff*diff;
  if(sum > r_squared) // If the x+y distance is too big...
    return false;
  diff=a[2]-b[2];
  sum+=diff*diff;
  if(sum > r_squared)
    return false;

  return true;
}

BTKVector
BTK::MATH::
set_vector_from_dihedral(BTKVector const & v3,
                         BTKVector const & v2,
                         BTKVector const & v1,
                         double len34,
                         double ang234,
                         double dih1234)
{
  //  1) build rhand coordinate axes such that z || v3-v2 and y || z X v1-v2
  //     note that the assignment of the y axis checks against v3-v2||v1-v2
  BTKVector z(v3-v2);
  normalize(z);

  BTKVector y(v1-v2);
  y = cross(z,y);
  normalize(y);

  BTKVector x(cross(y,z));

  //  2) find unit vector of v4-v3, and determine position of v4
  return v3+( len34 *
              ( ( x * cos(dih1234) + y * sin(dih1234) ) * sin(ang234) - z * cos(ang234) ));
}


//
//
BTKVector
BTK::MATH::
set_vector_from_two_angles(BTKVector const & v3,
                           BTKVector const & v2,
                           BTKVector const & v1,
                           double len34,
                           double ang234,
                           double ang134)
{
  // Chris Saunders - 4-2004
  // Build vector from two angles w/o explicit rotation:
  //
  // start with a convenient rh coordinate system:
  // y || v2-v3
  // z || cross(v1-v3,y)
  // x || cross(y,z)
  //

  BTKVector y(v2-v3);
  normalize(y);

  // need v1-v3 unit vector later:
  BTKVector v13_unit( v1-v3 );
  normalize( v13_unit );

  BTKVector z( cross(v13_unit,y) );
  normalize(z);

  BTKVector x( cross(y,z) );

  // solve for the projection of the v43 unit vector onto each axis:
  //

  // dot(v43_unit,v23_unit) = v43_para_v23 = v43_unit_y
  //
  double v43_unit_y = cos(ang234);

  //        dot(v43_unit,v13_unit) = v43_para_v13
  //  so..  v43_unit_y*v13_unit_y + v43_unit_x*v13_unit_x = v43_para_v13
  //  and solve for v43_unit_x:
  //
  double v43_unit_x = ( cos(ang134) - v43_unit_y*inner_prod(v13_unit,y) ) / inner_prod(v13_unit,x);

  // The two possible z solutions are resolved by always selecting the
  // positive z and instructing the function caller to order v1 and v2
  // appropriately:
  //
  double v43_unit_z = std::sqrt( 1.0 - (v43_unit_x*v43_unit_x) - (v43_unit_y*v43_unit_y) );

  BTKVector v43( len34 * (v43_unit_x*x + v43_unit_y*y + v43_unit_z*z) );

  return v43 + v3;
}
